from machine import Pin, PWM
import time


class Servo_180:
    def __init__(self, pin_num):
        self.freq = 50
        self.pwm = PWM(Pin(pin_num), freq=self.freq, duty=0)

    def write_angle(self, angle=0):
        """
        angle 0-180
        """
        angle = angle % 180

        duty = int((angle / 90 + 0.5) / 20 * 1023)
        self.pwm.duty(duty)


class Servo_360:
    def __init__(self, pin_num):
        self.freq = 50
        self.pwm = PWM(Pin(pin_num), freq=self.freq, duty=0)

    def run(self, speed=1):
        """
        speed: 速度 [-100, +100]
        """
        if speed < -100 or speed > 100:
            print("speed between -100 and 100")
            return

        us = 1.5  # 默认不转动
        if speed > 0 and speed <= 100:
            us = 0.5 + 1 * (100 - speed) / 100

        if speed < 0:
            us = 1.5 - 1 * speed / 100

        # print(us)
        duty = int(us / 20 * 1023)
        self.pwm.duty(duty)

    def stop(self):
        self.pwm.duty(0)


if __name__ == "__main__":
    degree = 180

    if degree == 180:
        servo = Servo_180(4)
        servo.write_angle(0)
        time.sleep(1)
        servo.write_angle(45)
        time.sleep(1)
        servo.write_angle(angle=90)

    if degree == 360:
        servo = Servo_360(4)
        servo.run(100)
        time.sleep(3)
        servo.run(-100)
        time.sleep(3)
        servo.stop()
